# Untitled - By: 86180 - 周一 7月 18 2022

import sensor, image, time, math, json,lcd,pyb
from pyb import millis
from math import pi, isnan
from pyb import Pin, Timer
from pyb import Pin
from pyb import UART
import os, tf, math, uos, gc
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_windowing((10,30,320,180))
sensor.set_vflip(True)
sensor.set_hmirror(True)
sensor.skip_frames(time = 100)
red_led = pyb.LED(1)
green_led = pyb.LED(2)
blue_led = pyb.LED(3)
#led1.on()
clock = time.clock()
###########神经网络##########################
net = None
labels = None
min_confidence = 0.7

try:
    # load the model, alloc the model file on the heap if we have at least 64K free after loading
    net = tf.load("trained.tflite", load_to_fb=uos.stat('trained.tflite')[6] > (gc.mem_free() - (64*1024)))
except Exception as e:
    raise Exception('Failed to load "trained.tflite", did you copy the .tflite and labels.txt file onto the mass-storage device? (' + str(e) + ')')

try:
    labels = [line.rstrip('\n') for line in open("labels.txt")]
except Exception as e:
    raise Exception('Failed to load "labels.txt", did you copy the .tflite and labels.txt file onto the mass-storage device? (' + str(e) + ')')

colors = [ # Add more colors if you are detecting more than 7 types of classes at once.
    (255,   0,   0),
    (  0, 255,   0),
    (255, 255,   0),
    (  0,   0, 255),
    (255,   0, 255),
    (  0, 255, 255),
    (255, 255, 255),
]
####################################################


thresholds=[
(83, 100, -22, 2, 25, 36),
(0, 40, -26, 5, -6, 23),

]

#基础部分复位
ch1zhi = 1450#左右移动
ch2zhi = 900#上下移动
#ch1zhi = 1111#左右移动
#ch2zhi = 1309#上下移动
#指示数字
###数字4
ch1zhi_4 = 1110#左右移动
ch2zhi_4 = 1000#上下移动
###数字3
ch1zhi_3 = 1110#左右移动
ch2zhi_3 = 1100#上下移动
###数字2
ch1zhi_2 = 1110#左右移动
ch2zhi_2 = 1200#上下移动
###数字1
ch1zhi_1 = 1130#左右移动
ch2zhi_1 = 1320#上下移动

#按键
KEY_nd = Pin('P0',Pin.IN,Pin.PULL_UP)#校正确定键
flag_nd = 0
nd=0
KEY_rest = Pin('P1',Pin.IN,Pin.PULL_UP)#复位
flag_rest = 0
rest=0
KEY_mode = Pin('P2',Pin.IN,Pin.PULL_UP)#循迹外
flag_mode = 0
mode=0
#鼠标
SL = Pin('P3', Pin.OUT_PP)#左键
SR = Pin('P4', Pin.OUT_PP)#右键
SL.high()
SR.high()

#舵机控制利用20ms的周期即50Hz
tim = Timer(4, freq=50) # Frequency in Hz
## 生成1kHZ方波，使用TIM4，channels 1 and 2分别是 50% 和 75% 占空比。
ch1 = tim.channel(1, Timer.PWM, pin=Pin("P7"), pulse_width=ch1zhi)#上
ch2 = tim.channel(2, Timer.PWM, pin=Pin("P8"), pulse_width=ch2zhi)


class PID:
    _kp = _ki = _kd = _integrator = _imax = 0
    _last_error = _last_derivative = _last_t = 0
    _RC = 1/(2 * pi * 20)
    def __init__(self, p=0, i=0, d=0, imax=0):
        self._kp = float(p)
        self._ki = float(i)
        self._kd = float(d)
        self._imax = abs(imax)
        self._last_derivative = float('nan')

    def get_pid(self, error, scaler):
        tnow = millis()
        dt = tnow - self._last_t
        output = 0
        if self._last_t == 0 or dt > 1000:
            dt = 0
            self.reset_I()
        self._last_t = tnow
        delta_time = float(dt) / float(1000)
        output += error * self._kp
        if abs(self._kd) > 0 and dt > 0:
            if isnan(self._last_derivative):
                derivative = 0
                self._last_derivative = 0
            else:
                derivative = (error - self._last_error) / delta_time
            derivative = self._last_derivative + \
                                     ((delta_time / (self._RC + delta_time)) * \
                                        (derivative - self._last_derivative))
            self._last_error = error
            self._last_derivative = derivative
            output += self._kd * derivative
        output *= scaler
        if abs(self._ki) > 0 and dt > 0:
            self._integrator += (error * self._ki) * scaler * delta_time
            if self._integrator < -self._imax: self._integrator = -self._imax
            elif self._integrator > self._imax: self._integrator = self._imax
            output += self._integrator
        return output
    def reset_I(self):
        self._integrator = 0
        self._last_derivative = float(2.5)




def find_max(blobs):
    max_size=0
    for blob in blobs:
        if blob[2]*blob[3] > max_size:
            max_blob=blob
            max_size = blob[2]*blob[3]
    return max_blob


pan_pid=PID(p=0.01,i=0,d=0,imax=90)
tilt_pid= PID(p=0.01,i=0,d=0,imax=90)  #脱机调试
pan_pid_mark=PID(p=1.10,i=0,d=0,imax=90)
tilt_pid_mark= PID(p=0.81,i=0,d=0,imax=90)  #脱机调试


def plane_get(img):
    plane_get=5
    for i, detection_list in enumerate(net.detect(img, thresholds=[(math.ceil(min_confidence * 255), 255)])):
        if (i == 0): continue # background class
        if (len(detection_list) == 0): continue # no detections for this class?

        print("********** %s **********" % labels[i])
        for d in detection_list:
            [x, y, w, h] = d.rect()
            center_x = math.floor(x + (w / 2))
            center_y = math.floor(y + (h / 2))
            print('x %d\ty %d' % (center_x, center_y))
            img.draw_circle((center_x, center_y, 12), color=colors[i], thickness=4)
            img.draw_string(center_x+12 ,center_y+12,labels[i],color=(255,0,0),scale =5 )
            plane_get=labels[i]
    return plane_get


def PWM_turn(x,y):
    ch1zhi_now=-2.96*x+1947.84#左右
    ch2zhi_now=-3.34*y+1465.57#上下
    ch1.pulse_width(int(ch1zhi_now))
    ch2.pulse_width(int(ch2zhi_now))
    print(22222222222222222222222222)
    SR.low()
    time.sleep_ms(300)
    print(333333333333333333333333333333333)
    SR.high()
    #time.sleep_ms(600)
    time.sleep(1)
    SL.low()
    time.sleep_ms(600)
    print(333333333333333333333333333333333)
    SL.high()
    rest=0



def target_get(img):
    blob=img.find_blobs([(92, 100, -22, 10, 1, 45)])
    if(blob):
        blob_max=find_max(blob)
        img.draw_rectangle(blob_max.rect(), color=(0,255,0))
        PWM_turn(blob_max.x(),blob_max.y())


#按键
def key_scan():
    global flag_rest,flag_mode,flag_nd,mode,rest,nd
    #发挥模式
    if((KEY_rest.value() == 0)and(flag_rest == 1)):#如果按键按下并且flag等于1
        flag_rest = 0#flag赋值为0
        rest = rest+1
        if(rest>1):
            rest=0
        #print(i)
    if((KEY_rest.value() == 1)and(flag_rest == 0)):#如果按键没有按下并且flag赋值为0
        flag_rest = 1#flag赋值为1
    #空白
    if((KEY_mode.value() == 0)and(flag_mode == 1)):#如果按键按下并且flag等于1
        flag_mode = 0#flag赋值为0
        mode = mode+1
        if(mode>1):
            mode=0
        #print(i)
    if((KEY_mode.value() == 1)and(flag_mode == 0)):#如果按键没有按下并且flag赋值为0
        flag_mode = 1#flag赋值为1
    #基础模式
    if((KEY_nd.value() == 0)and(flag_nd == 1)):#如果按键按下并且flag等于1
        flag_nd = 0#flag赋值为0
        nd = nd+1
        if(nd>1):
            nd=0
        #print(i)
    if((KEY_nd.value() == 1)and(flag_nd == 0)):#如果按键没有按下并且flag赋值为0
        flag_nd = 1#flag赋值为1

    print("111111111111111111111111111111111",mode,rest,nd)




while(True):
    img = sensor.snapshot().lens_corr(1.5)
    key_scan()
    #plane=plane_get(img)
    #print(plane)
    if(nd==1):
        ch1.pulse_width(int(ch1zhi))
        ch2.pulse_width(int(ch2zhi))
        print(22222222222222222222222222)
        SL.low()
        time.sleep_ms(600)
        print(333333333333333333333333333333333)
        SL.high()
        #time.sleep_ms(600)
        while(True):
            img = sensor.snapshot().lens_corr(1.5)
            plane=plane_get(img)
            print(plane)
            #if plane!="5":
            if plane=="1":
                ch1.pulse_width(int(ch1zhi_1))
                ch2.pulse_width(int(ch2zhi_1))
                print(22222222222222222222222222)
                SR.low()
                time.sleep_ms(300)
                print(333333333333333333333333333333333)
                SR.high()
                #time.sleep_ms(600)
                time.sleep(3)
                #SL.low()
                #time.sleep_ms(600)
                #print(333333333333333333333333333333333)
                #SL.high()
                #time.sleep_ms(600)
                nd=0
                break
                #return
            if plane=="2":
                ch1.pulse_width(int(ch1zhi_2))
                ch2.pulse_width(int(ch2zhi_2))
                print(22222222222222222222222222)
                SR.low()
                time.sleep_ms(300)
                print(333333333333333333333333333333333)
                SR.high()
                #time.sleep_ms(600)
                time.sleep(3)
                #SL.low()
                #time.sleep_ms(600)
                #print(333333333333333333333333333333333)
                #SL.high()
                #time.sleep_ms(600)
                nd=0
                break
                #return
            if plane=="3":
                ch1.pulse_width(int(ch1zhi_3))
                ch2.pulse_width(int(ch2zhi_3))
                print(22222222222222222222222222)
                SR.low()
                time.sleep_ms(300)
                print(333333333333333333333333333333333)
                SR.high()
                #time.sleep_ms(600)
                time.sleep(3)
                #SL.low()
                #time.sleep_ms(600)
                #print(333333333333333333333333333333333)
                #SL.high()
                #time.sleep_ms(600)
                nd=0
                break
                #return
            if plane=="4":
                ch1.pulse_width(int(ch1zhi_4))
                ch2.pulse_width(int(ch2zhi_4))
                print(22222222222222222222222222)
                SR.low()
                time.sleep_ms(300)
                print(333333333333333333333333333333333)
                SR.high()
                #time.sleep_ms(600)
                time.sleep(3)
                #SL.low()
                #time.sleep_ms(600)
                #print(333333333333333333333333333333333)
                #SL.high()
                #time.sleep_ms(600)
                nd=0
                break
                #return
    if(rest==1):
        ch1.pulse_width(int(ch1zhi))
        ch2.pulse_width(int(ch2zhi))
        count=0
        print(22222222222222222222222222)
        SL.low()
        time.sleep_ms(600)
        print(333333333333333333333333333333333)
        SL.high()
        #time.sleep_ms(600)
        while(True):
            img = sensor.snapshot().lens_corr(1.5)
            blob=img.find_blobs([(92, 100, -22, 10, 1, 45)])
            if(blob):
                blob_max=find_max(blob)
                img.draw_rectangle(blob_max.rect(), color=(0,255,0))
                count=count+1
            if(count>100):
                print("&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&")
                conut=0
                ch1zhi_now=-2.96*blob_max.x()+1947.84#左右
                ch2zhi_now=-3.34*blob_max.y()+1465.57#上下
                ch1.pulse_width(int(ch1zhi_now))
                ch2.pulse_width(int(ch2zhi_now))
                print(22222222222222222222222222)
                SR.low()
                time.sleep_ms(300)
                print(333333333333333333333333333333333)
                SR.high()
                #time.sleep_ms(600)
                time.sleep(3)
                #SL.low()
                #time.sleep_ms(600)
                #print(333333333333333333333333333333333)
                #SL.high()
                rest=0
                break
